#include "app_speed_pid.h"
#include "lcd/lcd.h"
#include "motor/hw_encoder.h"
#include "motor/hw_motor.h"
#include "pid.h"

PID speed_pid;

void speed_pid_init(void)
{
    pid_init(&speed_pid, 35, 6, 10, MOTOR_PWM_MAX, MOTOR_PWM_MAX, 68);
}

PID *get_speed_pid(void)
{
    return &speed_pid;
}

PID motor_speed_control(int target_speed)
{
    int PWM;
    // 传入PID静态参数、目标值（目标速度）、当前值（当前编码器获取的实时速度）
    // PID输出的值为控制信号，传入到PWM变量中
    PWM = pid_calc(&speed_pid, target_speed, get_encoder_count());
    // 控制信号转为电机实际控制值
    if (PWM > 0) // 控制信号大于0
    {
        // set_motor(0, PWM); // 顺时针旋转，pwm为旋转速度
        set_motor(PWM, 0);
    }
    else if (PWM < 0)
    {
        PWM = -PWM;        // 将负数转为正数（例如PWM=-10，则-（-10）=10）
        // set_motor(PWM, 0); // 逆时针旋转，pwm为旋转速度
        set_motor(0, PWM);
    }
    return speed_pid;
}